#include "DriveTrain.h"

DriveTrain::DriveTrain()
{
	driveLeft = new Jaguar(1);
	driveRight = new Jaguar(2);

	encLeft = new Encoder(1, 2);
	encRight = new Encoder(3, 5);
	m_LCD = DriverStationLCD::GetInstance();

	pidY = new PIDController(0.1, 0.001, 0.0, encLeft, driveLeft);

	driveLeft->SetExpiration(.1);
	driveRight->SetExpiration(.1);
}

void DriveTrain::OpStart()
{
	driveLeft->SetSafetyEnabled(true);
	driveRight->SetSafetyEnabled(true);

	a = 0.5;
}

void DriveTrain::AutoStart()
{
	driveLeft->SetSafetyEnabled(false);
	driveRight->SetSafetyEnabled(false);

	pidY->Enable();
}

void DriveTrain::OpControl(float x, float y)
{
	targetY = a * (y * y * y) + (1 - a) * y;
	targetX = a * (x * x * x) + (1 - a) * x;
	driveLeft->Set((targetY - targetX) * 0.5);
	driveRight->Set((targetY + targetX) * -0.5);
}

void DriveTrain::AutoStraight(float target)
{
	while (DistX < target)
	{
		DriveTrain::DeadUpdate();
		pidY->SetSetpoint(target / TickstoFeet);
		driveRight->Set(driveLeft->Get() * -1);
	}
}

void DriveTrain::AutoTurn(float target)
{
	while (theta * (180 / pi) < target)
	{
		DriveTrain::DeadUpdate();
		pidY->SetSetpoint(2 * pi * (Rw / Dw) * ((target + target) / Tr));
		driveRight->Set(driveLeft->Get());
	}
}

void DriveTrain::DeadStart()
{
	encLeft->Start();
	encRight->Start();
	encLeft->Reset();
	encRight->Reset();

	E1 = 0;
	E2 = 0;
	DistX = 0;
	DistY = 0;
	theta = 0;

	Dt = 0;
	Dy = 0;
	Dx = 0;
	T1 = 0;
	T2 = 0;
}

void DriveTrain::DeadReset()
{
	encLeft->Reset();
	encRight->Reset();

	E1 = 0;
	E2 = 0;
	DistX = 0;
	DistY = 0;
	theta = 0;
}

void DriveTrain::DeadStop()
{
	encLeft->Stop();
	encRight->Stop();
}

void DriveTrain::DeadUpdate()
{
	T1 = encLeft->Get() - E1;
	T2 = encRight->Get() - E2;

	Dt = 2 * pi * (Rw / Dw) * ((T1 - T2) / Tr);
	theta += Dt;

	if (theta > pi * 2)
	{
		theta -= pi * 2;
	}
	else if (theta < 0)
	{
		theta += pi * 2;
	}

	Dx = Rw * cos(theta) * (T1 + T2) * (pi / Tr);
	Dy = Rw * sin(theta) * (T1 + T2) * (pi / Tr);

	DistX += Dx;
	DistY += Dy;

	E1 = encLeft->Get();
	E2 = encRight->Get();
}

void DriveTrain::DeadDisplay()
{
	m_LCD->Printf(DriverStationLCD::kUser_Line1, 2, "Left: %5i     ", encLeft->Get());
	m_LCD->Printf(DriverStationLCD::kUser_Line2, 2, "Right: %5i    ", encRight->Get());

	m_LCD->Printf(DriverStationLCD::kUser_Line3, 2, "X: %f feet     ", (DistX
			/ toFeet));
	m_LCD->Printf(DriverStationLCD::kUser_Line4, 2, "Y: %f feet     ", (DistY
			/ toFeet));
	m_LCD->Printf(DriverStationLCD::kUser_Line5, 2, "Angle: %i degrees   ", (int) (theta
			* (180 / pi)));
	m_LCD->UpdateLCD();
}
